//
// Created by vslab on 23-4-9.
//
#define DEBUG_
#include "../include/msgs.h"
#include <iostream>
#include <sstream>
#include <string>

using namespace std;


namespace GNSS{
    namespace msg{
        void Satellite::reset()
        {
            elevation = 0.0;
            azimuth = 0.0;
            snr = 0.0;
        }

        double Base_msg::number_trans(string num_string)
        {
//            return std::stod(num_string);
///           cout << std::stod(num_string) << endl;
            double num;
            stringstream ss;
            ss.precision(num_string.length());
            ss << num_string;
            ss >> num;
            return num;
        }

        Base_msg::Base_msg() :
        field_nums_(0),
        is_valid_(false),
        start_(0),
        end_(0)
        { }

        Gga::Gga() :
        UTC_(UTC()), lat_(latlon()), lon_(latlon()),
        lat_dir_(latlon_dir()), lon_dir_(latlon_dir()), sate_nums_(sate_num())
        { }


        bool Gga::check()
        {
            if (is_valid_ == false)
                return false;
            else
            {

            }

            return true;
        }

        void Gga::parse(string raw_data)
        {
            try {
                int idx = 1;
                while (raw_data.at(end_ + 1) != end_flag_ && end_ != -1)
                {
                    start_ = raw_data.find(separator_, start_ + 1);
                    size_t start = start_ + 1;
                    end_ = raw_data.find(separator_, start_ + 1);
                    double raw_num = 0.0;
                    string data_sub = raw_data.substr(start, end_ - start);
                    switch(idx) {
                        case 1: {
                            raw_num = number_trans(data_sub);
                            int hh = raw_num / 10000;
                            int mm = raw_num / 100 - hh * 100;
                            int ss = raw_num - hh * 10000 - mm * 100;
                            UTC_.hh = hh;
                            UTC_.mm = mm;
                            UTC_.ss = ss;
                            UTC_.exp = to_string(hh) + ":" + to_string(mm) + ":" + to_string(ss);
                            UTC_.bj_time = to_string(hh + 8) + ":" + to_string(mm) +":" + to_string(ss);
                            break;
                        }
                        case 2: {
                            raw_num = number_trans(data_sub);
                            int dd = raw_num / 100;
                            double mm = raw_num - dd * 100;
                            lat_.dd = dd;
                            lat_.mm = mm;
                            lat_.exp = dd + mm / 60;
                            break;
                        }
                        case 3: {
                            lat_dir_ = data_sub;
                            break;
                        }
                        case 4:
                        {
                            raw_num = number_trans(data_sub);
                            int dd = raw_num / 100;
                            double mm = raw_num - dd * 100;
                            lon_.dd = dd;
                            lon_.mm = mm;
                            lon_.exp = dd + mm / 60;
                            break;
                        }
                        case 5:
                            lon_dir_ = data_sub;
                            break;
                        case 7:
                            sate_nums_ = number_trans(data_sub);
                            break;
                    }
                    ++idx;
                }
            }
            catch(std::out_of_range e)
            {
                is_valid_ = false;
                cout << "frame error! drop" << endl;
            }
        }

        Gsv::Gsv() :
        sentence_num_(0), no_(0), visible_sate_num_(0)
        { }

        bool Gsv::check() { return true; }

        void Gsv::parse(std::string raw_data)
        {
            try
            {
                int idx = 1;
                Satellite sat;
                while(raw_data.at(end_ + 1) != end_flag_ && end_ != -1)
                {
                    start_ = raw_data.find(separator_, start_ + 1);
                    size_t start = start_ + 1;
                    end_ = raw_data.find(separator_, start_ + 1);
                    string data_sub = raw_data.substr(start, end_ - start);

                    switch (idx) {
                        case 1: {
                            sentence_num_ = number_trans(data_sub);
                            break;
                        }
                        case 2: {
                            no_ = number_trans(data_sub);
                            break;
                        }
                        case 3: {
                            visible_sate_num_ = number_trans(data_sub);
                            break;
                        }
                        case 4:
                        {
                            sat.reset();
                            break;
                        }
                        case 5:
                        {
                            sat.elevation = number_trans(data_sub);
                            break;
                        }
                        case 6:
                        {
                            sat.azimuth = number_trans(data_sub);
                            break;
                        }
                        case 7:
                        {
                            if (data_sub != "")
                            {
                                sat.snr = number_trans(data_sub);
                            }
                            else
                            {
                                sat.snr = -1.0;//non valid
                            }
                            satellites_.push_back(sat);
                            idx = 3;
                            break;
                        }
                    }
                    ++idx;
                }
            }
            catch (out_of_range e)
            {
                is_valid_ = false;
                cout << "frame error! drop" << endl;
            }
        }

        Vtg::Vtg() :
        yaw_true_north_(0), vel_(0)
        { }

        bool Vtg::check() { return true; }

        void Vtg::parse(std::string raw_data)
        {
            try
            {
                int idx = 1;
                while(raw_data.at(end_ + 1) != end_flag_ && end_ != -1)
                {
                    start_ = raw_data.find(separator_, start_ + 1);
                    size_t start = start_ + 1;
                    end_ = raw_data.find(separator_, start_ + 1);
                    string data_sub = raw_data.substr(start, end_ - start);
                    switch (idx)
                    {
                        case 1:
                        {
                            yaw_true_north_ = number_trans(data_sub);
                            break;
                        }
                        case 7:
                        {
                            vel_ = number_trans(data_sub);
                            break;
                        }
                    }
                    ++idx;
                }
            }
            catch (out_of_range e)
            {
                is_valid_ = false;
                cout << e.what() << endl;
                cout << "frame error, drop" << endl;
            }
        }
    }
}